//********************************************************************************************
//  QC-Helper zur QuadroControl III  &  QuadroControl II / XS 
//  (c) 2011 Olaf Jaster. Alle Rechte vorbehalten.
//  TWI-Routinen und UART-Routinen urspruenglich von Peter Fleury - fuer eigene Zwecke angepasst
//  http://jump.to/fleury
//********************************************************************************************
//  QuadroControl III  &  QuadroControl II / XS 
//  Universelle Multicoptersteuerungen
//
//  (c) 2007-2010 by Tido Tebben. Alle Rechte vorbehalten.
//********************************************************************************************
//  http://www.qc-copter.de
//********************************************************************************************
//  Datei:			qchelper.c - Hauptprogramm fuer QC-Helper-Platine
//					Im ersten Schritt Hoehenkontrolle via Ultraschall
//********************************************************************************************
//
// Nutzungsbedingungen:
//
// Die Projekte QuadroControl III sowie QuadroControl II/XS (nachfolgend "QuadroControl" 
// genannt, inklusive Hardware, Software, Quellcodes, Dokumentation, HEX-Dateien) sind 
// ausschliesslich fuer den nicht - kommerziellen Einsatz zulaessig. 
// Der kommerzielle Nachbau und/oder die kommerzielle Verwertung der hier bereitgestellten 
// Informationen sind untersagt. 
//
// Eine kommerzielle Nutzung ist z.B. der Verkauf von QuadroControl Hardware oder 
// Bausaetzen, Verkauf von Quadrocoptern auf Basis der QuadroControl, Anfertigung und 
// Verkauf von Luftaufnahmen, etc.
//
// Die Portierung des Quellcodes auf andere Hardware als die von http://www.qc-copter.de 
// bereitgestellte ist nur mit der ausdruecklichen Erlaubnis von Tido Tebben.
//
// Die Verwendung und Veroeffentlichung des Quellcodes oder der Dokumentation (auch auszugsweise) 
// in anderen Projekten und/oder auf anderen Webseiten oder anderen Medien unterliegt ebenfalls 
// diesen Nutzungsbedingungen. Die Seite "http://www.qc-copter.de" muss dabei als Ursprung der 
// Information eindeutig genannt werden.
//
// Da bei der Entwicklung von Software und Hardware Fehler nie ganz ausgeschlossen werden koennen, 
// weise ich hiermit darauf hin, dass ich keinerlei Garantie fuer Schaeden, die durch den Nachbau 
// und den Gebrauch der QuadroControl und / oder der Dokumentation entstehen, 
// uebernehme. Weiterhin uebernehme ich keine Garantie fuer Folgeschaeden, wie Personenschaeden, 
// entgangene Gewinne, Vermoegensverluste oder anderer mittelbarer und unmittelbarer Schaeden, 
// die durch den Gebrauch oder die Nichtverwendbarkeit der QuadroControl, der 
// Quelltexte und/oder der Dokumentation entstehen. Dies gilt auch dann, wenn ich ueber die 
// Moeglichkeit solcher Schaeden unterrichtet war oder bin. 
//
//********************************************************************************************

#include <avr/io.h>
#include <util/delay.h>
#include <compat/twi.h>
#include <avr/interrupt.h>
#include <stdlib.h>

#define twi_buffersize 4
#define BAUD 57600				// Baudrate 57600 bps
#define address_qchelper 0x50	//Die Slave-Adresse fuer die QC-Helper-Platine

volatile uint8_t twi_data[twi_buffersize+1];
volatile uint8_t buffer_adr; //"Adressregister" fuer den Buffer
volatile uint8_t changes = 1;


void uart_send_char(unsigned char wert)  // Zeichen ueber UART ausgeben
{ 
    // warten bis Senden moeglich
	while (!(UCSR0A & 0x20))  
	{
	}
 	
	// Zeichen an UART senden
    UDR0 = wert;    
} 


void uart_send_string(const char *s )  // String ueber UART ausgeben
{
    while (*s) 
      uart_send_char(*s++);

}



void uart_send_integer( const int val )  // Integer-Zahlen ueber UART ausgeben
{
    char buffer[8];
    uart_send_string( itoa(val, buffer, 10) );

}


ISR(TWI_vect)
{
   static unsigned char i2c_state;
   unsigned char twi_status;   
   
   // Disable Global Interrupt
   cli();   // Get TWI Status Register, mask the prescaler bits (TWPS1,TWPS0)
 
 
  twi_status=TWSR & 0xF8;     

   switch(twi_status) {
     case TW_SR_SLA_ACK:      // 0x60: SLA+Write received, ACK returned
	   i2c_state=0;           // Start I2C State for Register Address required	 

       TWCR |= (1<<TWINT);    // Clear TWINT Flag
       break;     
	   
	 case TW_SR_DATA_ACK:     // 0x80: data received, ACK returned
		if (i2c_state == 0) {
			buffer_adr = TWDR;      // Save data to the register address
			i2c_state = 1;
		} else {
			twi_data[buffer_adr] = TWDR;      // Save to the register data
			buffer_adr += 1;
			i2c_state = 2;
			changes=1;
       }

       TWCR |= (1<<TWINT);    // Clear TWINT Flag
       break;     
	   
	 case TW_SR_STOP:         // 0xA0: stop or repeated start condition received while selected
		if (i2c_state == 2) {
		//	 i2c_slave_action(1); // Call Write I2C Action (rw_status = 1)
		i2c_state = 0;	      // Reset I2C State
       }	   

       TWCR |= (1<<TWINT);    // Clear TWINT Flag
       break;

     case TW_ST_SLA_ACK:      // 0xA8: SLA+Read received, ACK returned
 	 case TW_ST_DATA_ACK:     // 0xB8: data transmitted, ACK received
	   if (i2c_state == 1) {
	   		TWDR = twi_data[buffer_adr];
			buffer_adr += 1;
       }	   	  

       TWCR |= (1<<TWINT);    // Clear TWINT Flag
       break;     
	   
	 case TW_ST_DATA_NACK:    // 0xC0: data transmitted, NACK received
	 case TW_ST_LAST_DATA:    // 0xC8: last data byte transmitted, ACK received
     case TW_BUS_ERROR:       // 0x00: illegal start or stop condition
			i2c_state = 0;	      // Reset I2C State
     
	 default:
       TWCR |= (1<<TWINT);    // Clear TWINT Flag
       i2c_state = 0;         // Back to the Begining State
   }   // Enable Global Interrupt
   sei();


}


int Hoehe_ermitteln (void)
{
	volatile unsigned char echo_on;          // Zeitspeicherung bei Echo on
	volatile unsigned int echo_off;          // Zeitspeicherung bei Echo off
	volatile unsigned int distance;			// Entfernung in cm

	TCCR1B = 0x00;				// Timer 1 Normaler Betrieb, Timer stoppt
 	TIMSK0 |= (1 << TOIE1);		// T1 Overflow Interrupt frei
 	sei();						// Globale Interruptfreigabe
 	TCCR1B = 0x03;				// Timer startet; Teiler 64
	TCNT1 = 0x00;				// Zaehler-Startwert = 0
	DDRD |= (1 << PD7);			// Pin D7 auf Ausgang stellen
	PORTD |= (1 << PD7);		// Triggerimpuls an
	_delay_us(15);				// 15 Mikrosekunden Impulsdauer
    PORTD &= (~(1 << PD7));		// Triggerimpuls aus
	DDRD &= (~(1 << PD7));		// Pin D7 auf Eingang umstellen
    
   	while ((!(PIND & (1 << PD7))) && (TCNT1 < 110));		// Auf Beginn Echo warten - nach spaetestens 110 Timerintervallen muss Echo anfangen, ansonsten liegt ein Fehler vor
    echo_on = TCNT1;            		// ersten Wert speichern
	if (echo_on < 110)
	{
    	while ((PIND & (1 << PD7)) && (TCNT1 < 4000));       	// auf Ende Echo warten - nach spaetestens 4000 Timerintervallen muss Echo aufhoeren, ansonsten zu hoch oder ausgefallen
    	echo_off = TCNT1;            		// zweiten Wert speichern
		
		// Distanz = 1 cm / 58 us
		// Beispiel: 	Ergebnis: 750 Takte
		//				Teiler: 64
		//				Tatsaechliche Takte: 750 * 64 / 8 (Mhz) = 6000
		//				6000 us / 58 us/cm = 103,4 cm
		// Berechnung:  Hoehe = (echo_off-echo_on) * Teiler / (clq_freq / 1000000) / 58
		// 				Hoehe = (echo_off-echo_on) * 64 / 8 / 58
		// 				Hoehe = (echo_off-echo_on) * 8 / 58
		//				Hoehe = (echo_off-echo_on) * 4 / 29
		// Beispiel: 100 cm = 5800 us = 725 Takte bei Teiler 64 und 8 MHz CPU-Takt
		// Maximalhoehe lt. Datenblatt ca. 400 cm

		distance = (echo_off - echo_on)*4/29;  // Maximale Hoehe ca. 400 cm

		uart_send_string("\r\nHoehe: ");
		uart_send_integer(distance);
		uart_send_string(" cm");
		uart_send_string("\r\n");
	}
	
	return(distance);
	
}



int main(void)
{
  
	cli();
	// LED-Ports auf Ausgang
	DDRC = 0xFF; 
	DDRB = 0xFF; 
	DDRD   = 0b11111100;		//Port D Bit 2-7 auf Ausgang - D7 = Ultraschall-Sensor kombinierter Ein/Ausgang

	TWAR = address_qchelper & 0xFE; // TWI Listening Adresse setzen
	TWDR = 0x00;                 // Default Initial Value  
  
	// Start Slave Listening: Clear TWINT Flag, Enable ACK, Enable TWI, TWI Interrupt Enable
	TWCR = (1<<TWINT) | (1<<TWEA) | (1<<TWEN) | (1<<TWIE);  

	
	// Enable Global Interrupt
	sei();

  	// serielle Schnittstelle konfigurieren auf 57.600 bps, nur Sendebetrieb
	UBRR0H = 0; 
	UBRR0L = 8; // (8.000.000 Hz + (8 * 57.600) ) / (16 * 57.600) -1     abgeschnitten auf Ganzzahl!
	UCSR0B |= (1<<TXEN0); // Transmit enable

	// Initial Variable Used
 	buffer_adr=0;


	// Testdaten in Pufferbereich schreiben
	for (uint8_t i=0; i<twi_buffersize;i++)
	{
		twi_data[i]=i*10;
	}

  
	while (1) {
		// Wurden neue Werte per Interruptroutine uebertragen?
		if (changes) {
			PORTC |= 0b00000100; // Bit 2 setzen - rote LED leuchtet - Blinken zur Kontrolle, ob uC haengt
			PORTC &= 0b11111101; // Bit 1 loeschen - gelbe LED aus - Blinken zur Kontrolle, ob uC haengt
 			uart_send_string("twi_data (Slave "); uart_send_integer(address_qchelper); uart_send_string("):\r\n");
			for(uint8_t i=0;i<twi_buffersize;i++)
			{
				uart_send_integer(twi_data[i]);
				uart_send_string("\r\n");
			}
			uart_send_string("\r\n");//leerzeile

			// Bufferbeschreibung:
			// Byte 0, Bit 0: Hoehenregelung aktivieren?
			// Byte 1: Knueppelstellung Gashebel
			// Byte 2: Dummy fuer Lesekontrolle
			// Byte 3: Gaswert aus PD-Hoehenregelung

			twi_data[3]=Hoehe_ermitteln()/2; 	// Testweise Hoehe/2 als Gaswert uebergeben
												// Byte 2 wird von QC-III dann mit Wert von Byte 3*2 ueberschrieben und 
												// bei nächster Messung zurueckgeliefert (also in etwa Hoehe in cm
												// ausser bei Uberlauf, also Hoehe > 255 cm
			changes=0;

			PORTC |= 0b00000010; // Bit 1 setzen - gelbe LED leuchtet - Blinken zur Kontrolle, ob uC haengt
			PORTC &= 0b11111011; // Bit 2 loeschen - rote LED aus - Blinken zur Kontrolle, ob uC haengt
		}

  	}
  
}
